#include "task_core.h"

#define MAXNUM_SEMAPHORE		8
#define MAXNUM_SM_WAITTASK		(OSTASK_NUM-1)

typedef struct {
	int value;
	ostcb_t *ptask_occupy;
	unsigned long bottom;
	unsigned long top;
	ostcb_t *ptask_wait[MAXNUM_SM_WAITTASK];
} semaphore_t;

static int sm_used;
static semaphore_t sm_buf[MAXNUM_SEMAPHORE];

void os_sminit(void)
{
	int i, j;

	sm_used = 0;

	for(i=0; i<MAXNUM_SEMAPHORE; i++)
	{
		sm_buf[i].value = 0;
		sm_buf[i].ptask_occupy = NULL;

		sm_buf[i].top = sm_buf[i].bottom = 0;

		for(j=0; j<MAXNUM_SM_WAITTASK; j++)
			sm_buf[i].ptask_wait[j] = NULL;
	}
}

int os_smcreate(int init_val)
{
	int rtn;

	if(sm_used >= MAXNUM_SEMAPHORE) return -1;

	rtn = sm_used;

	sm_buf[rtn].value = init_val;
	sm_buf[rtn].ptask_occupy = NULL;
	sm_buf[rtn].top = sm_buf[rtn].bottom = 0;

	sm_used++;
	return rtn;
}

void os_smlock(int sm_id)
{
	semaphore_t *psm;
	unsigned char bottom;

	if(sm_id >= sm_used) return;

	psm = &(sm_buf[sm_id]);

	psm->value--;

	if(psm->value < 0)
	{
		bottom = psm->bottom;
		bottom++;
		if(bottom >= MAXNUM_SM_WAITTASK) bottom = 0;
		if(bottom == psm->top)
		{
			psm->value++;
			return;
		}

		psm->ptask_wait[psm->bottom] = gp_task_cur;
		psm->bottom = bottom;

		if(gp_task_cur == psm->ptask_occupy) return;

		gp_task_cur->ev_wait = 0;
		gp_task_cur->stat = TASKSTAT_BLOCK;

		OS_TASK_BLOCK();
	}
	else
	{
		psm->ptask_occupy = gp_task_cur;
	}
}

int os_smtrylock(int sm_id)
{
	semaphore_t *psm;	

	if(sm_id >= sm_used) return 1;

	psm = &(sm_buf[sm_id]);

	psm->value--;

	if(psm->value < 0)
	{		
		psm->value++;
		return 1;		
	}
	else
	{
		psm->ptask_occupy = gp_task_cur;
	}

	return 0;
}

void os_smunlock(int sm_id)
{
	semaphore_t *psm;
	unsigned char top;

	if(sm_id >= sm_used) return;

	psm = &(sm_buf[sm_id]);

	psm->value++;

	if(psm->top == psm->bottom)  //empty
	{
		psm->ptask_occupy = NULL;
		return;
	}

	top = psm->top;
	psm->ptask_wait[top]->stat = TASKSTAT_RUN;
	psm->ptask_occupy = psm->ptask_wait[top];
	top++;
	if(top >= MAXNUM_SM_WAITTASK) top = 0;
	psm->top = top;
}
